/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file ant_point.cxx
	\brief class implemenation for AntennaCmd

                    \author Maciej Stachura
             $Date: 2010/02/04 13:34:45 $
*/

#include <unistd.h>
#include <signal.h>
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>

#include "ant_point.h"
#include "color.h"

  /*<-------Defines--------->*/
    #define VAR_UPDATE_DIST 5000  // distance moved in meters to recompute variation
    #define TRACKING_CLIENT_TIMEOUT 10.0   //seconds
    #define TRACKING_LOOP_PERIOD     0.5   //seconds

		#define MAG_SEND_PERIOD 1.0     // seconds
  /*<------End Defines------>*/

  /*<---Global Variables---->*/
    extern bool verbose;
		extern String piccoloGSCommIPStr;
		extern String piccoloGSCommPortStr;
  /*<-End Global Variables-->*/

AntennaCmd::AntennaCmd(String name, PointingType type, float range, uint8_t systype) : ThreadedPipe(name)
{
	this->omni_range = range;
	this->pointing_type = type;
	this->system_type = systype;

	PrintMode(STANDBY);
	tracker_mode = STANDBY;

  antenna.elevation = 0;
  antenna.azimuth = 0;
	antenna.mode = OMNI;
	antenna.gs_telem = 0;
	antenna.tr_telem = 0;
	antenna.mag_telem = 0;
	antenna.tr_back_telem = 0;
	antenna.pointing_type = pointing_type;

	manual_offset = 0;
	pause_tracking = false;
}

bool AntennaCmd::init(ShmStruct * shmStructPtr)
{
	ThreadedPipe::init(shmStructPtr);

	last_tracking_loop.stamp();
	last_client_update.stamp();
	last_mag_update.stamp();

	Client * me = shmPtr->clients.getClient(0);
	subscribe(CMD_MAG_TELEM, me, true, INBOARD_TX);
	subscribe(CMD_GPS_TELEM, me, true, INBOARD_TX);

	CMD_CMD(fifo_msg.data)  = CMD_ANT_TRACK;
	CMD_NUM(fifo_msg.data)  = sizeof(AntPoint_t);
	CopyStructToBuf(antenna, CMD_DATAPTR(fifo_msg.data) );
	CMD_CSUM(fifo_msg.data) = 0;
	CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

	fifo_msg.size = CMD_SIZE(fifo_msg.data);
	fifo_msg.dest.s_addr = shmPtr->clients.getAddr(requester).s_addr;
	fifo_msg.src.s_addr = shmPtr->clients.getAddr(me).s_addr;
	writeFifo(OUTBOARD_TX);
  
  return true;
}

void AntennaCmd::update() 
{
  int val, maxFD;
	fd_set readFDs, writeFDs;                   // fd sets
	struct timeval timeout;

  FD_ZERO(&readFDs);	                    		// zero read fd set
  FD_ZERO(&writeFDs);	                    		// zero write fd set
  FD_SET(fifo_FDs[INBOARD_RX],&readFDs);			// add fifo
  FD_SET(fifo_FDs[OUTBOARD_RX],&readFDs);			// add fifo
  FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);			// add fifo
  FD_SET(fifo_FDs[OUTBOARD_TX],&writeFDs);		// add fifo

  fifo_FDs[INBOARD_RX] > fifo_FDs[OUTBOARD_RX] ? maxFD = fifo_FDs[INBOARD_RX] : maxFD = fifo_FDs[OUTBOARD_RX];
  if(fifo_FDs[INBOARD_TX] > maxFD) maxFD = fifo_FDs[INBOARD_TX];
  if(fifo_FDs[OUTBOARD_TX] > maxFD) maxFD = fifo_FDs[OUTBOARD_TX];

  timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

  if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
    if( val != 0 ) {
      if (errno != EINTR) {
        perror("AntennaCmd::update() select");
        execRate = -1;
      } else
        cout << getName() << "::update() - select interrupted" << endl;
    }
  }
  else {
    if(FD_ISSET(fifo_FDs[OUTBOARD_RX], &readFDs))
    {
      int n = 0;
      n=readFifo(OUTBOARD_RX);
      if(n>0) {
        // process read commands
      }
    }

    if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs)) {
      int n = 0;
			n=readFifo(INBOARD_RX);
			if(n>0) {

				uint16_t length = CMD_SIZE(fifo_msg.data);
				if( length == fifo_msg.size) {

					Command cmd((unsigned char *)fifo_msg.data);
					if( !cmd.isValid() )
						cout << getName() << "::update() - error, invalid command" << endl << cmd ;
					else {
						parseInternalData(cmd);
					}
				}
      }
    }
				
    if(FD_ISSET(fifo_FDs[OUTBOARD_TX], &writeFDs)) {
			currentStamp.stamp();
			if((currentStamp - last_tracking_loop ) > (TRACKING_LOOP_PERIOD * SEC2MICRO) ) {
				if(tracker_mode == TRACK && !pause_tracking) {
					TrackTargets(-1.0,-1.0);
					last_tracking_loop.stamp();
				}
			}
		}
	}

	currentStamp.stamp();
	if((currentStamp - last_client_update ) > (TRACKING_CLIENT_TIMEOUT * SEC2MICRO) ) {
		antenna.tr_telem = 0;
		for(int i=0; i< tracked_vehicles.getNumClients(); i ++) {
			cout << "requesting TELEM stream" << endl;
			//subscribe(CMD_GPS_TELEM, tracked_vehicles.getClient(i), true, INBOARD_TX);
			subscribe(CMD_TELEM, tracked_vehicles.getClient(i), true, INBOARD_TX); // FIXME!!
		}
		last_client_update.stamp();
	}
	if((currentStamp - last_mag_update ) > (MAG_SEND_PERIOD * 2.5 * SEC2MICRO) ) {
		antenna.mag_telem = 0;
	}
}

void AntennaCmd::parseInternalData(const Command &cmd)
{
	Client * a_client = shmPtr->clients.getClient(fifo_msg.src);
	Client * me = shmPtr->clients.getClient(0);

	if(a_client == NULL) return;

	uint8_t * dataPtr = CMD_DATAPTR(fifo_msg.data);

  switch(cmd.cmd() ) {

		case CMD_TRACK: {
			int num_vehicles = cmd.size()/sizeof(VehicleInfo_t);
			int sub_sum = 0;

			requester = a_client;
			tracked_vehicles.clear();
			Client * tracked_client;

			if(num_vehicles == 0) {
				PrintMode(STANDBY);
				tracker_mode = STANDBY;
				antenna.tr_telem = 0;

				for(int i=0; i< tracked_vehicles.getNumClients(); i ++) {
					//subscribe(CMD_GPS_TELEM, tracked_vehicles.getClient(i), false, INBOARD_TX);
					Client * a_client = shmPtr->clients.getClient(tracked_vehicles.getClient(i)->name);
					if(a_client != NULL)
						a_client->state.gs_backup = 0;
					subscribe(CMD_TELEM, tracked_vehicles.getClient(i), false, INBOARD_TX);//FIXME!!
				}
				tracked_vehicles.clear();

				antenna.elevation = 0;
				antenna.azimuth = 0;
				antenna.mode = OMNI;
				antenna.tr_telem = 0;
				antenna.pointing_type = pointing_type;

				CMD_CMD(fifo_msg.data)  = CMD_ANT_TRACK;
				CMD_NUM(fifo_msg.data)  = sizeof(AntPoint_t);
				CopyStructToBuf(antenna, CMD_DATAPTR(fifo_msg.data) );
				CMD_CSUM(fifo_msg.data) = 0;
				CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

				fifo_msg.size = CMD_SIZE(fifo_msg.data);
				fifo_msg.dest.s_addr = shmPtr->clients.getAddr(requester).s_addr;
				fifo_msg.src.s_addr = shmPtr->clients.getAddr(me).s_addr;
				writeFifo(OUTBOARD_TX);

				CMD_CMD(fifo_msg.data)  = system_type;
				CMD_CSUM(fifo_msg.data) = 0;
				CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);
				writeFifo(INBOARD_TX);

				connectToGS(piccoloGSCommIPStr, piccoloGSCommPortStr, false);
			}
			if( num_vehicles > 0 ) {
				if((tracked_client = shmPtr->clients.getClient(((VehicleInfo_t*)dataPtr)[0].id)) != NULL) {
					if(tracked_vehicles.add(tracked_client)) {
						cout << "requresting TELEM stream" << endl;
						//subscribe(CMD_GPS_TELEM, tracked_client, true, INBOARD_TX);
						tracked_client->state.gs_backup = 1;
						subscribe(CMD_TELEM, tracked_client, true, INBOARD_TX); //FIXME!!
						sub_sum++;
					}
				}
				if( sub_sum > 0 ) {
					pause_tracking = false;

					fifo_msg.src.s_addr = shmPtr->clients.getAddr(a_client).s_addr;
					CMD_CMD(fifo_msg.data) = CMD_TRACK;
					ACK(INBOARD_TX);

					tracker_mode = TRACK;
					PrintMode(TRACK);

					connectToGS(piccoloGSCommIPStr, piccoloGSCommPortStr, true);

				} else {
					NACK(INBOARD_TX);
					tracked_vehicles.clear();
				}

			} else
				ACK(INBOARD_TX);
		} break;
		case CMD_MAG_TELEM: {
		  memcpy(&mag, dataPtr, sizeof(MagStruct_t));
			antenna.mag_telem = 1;
			last_mag_update.stamp();
      break;
      }

    case CMD_GPS_TELEM: {
			//GPSTelem_t * telemetry  = (GPSTelem_t*)dataPtr;
			Client * tracked_client = tracked_vehicles.getClient(a_client->name);
			if(tracked_client != NULL) {
				//memcpy(&(tracked_client->telemetry.gps), telemetry, sizeof(GPSTelem_t));
				antenna.tr_telem = 1;
				last_client_update.stamp();
			} else if(a_client == me) {
				if(!antenna.gs_telem && me->state.gps ){
					antenna.gs_telem = 1;
					mag_var = WMM_mag_var(me->telemetry.gps.pos.lat, me->telemetry.gps.pos.lon, me->telemetry.gps.pos.alt/1000.0, 2010);
					memcpy(&last_gps_update, &(me->telemetry.gps), sizeof(GPSTelem_t));
				}

				if(GPSRange(me->telemetry.gps.pos, last_gps_update.pos) > VAR_UPDATE_DIST){
					mag_var = WMM_mag_var(me->telemetry.gps.pos.lat, me->telemetry.gps.pos.lon, me->telemetry.gps.pos.alt/1000.0, 2010);
					memcpy(&last_gps_update, &(me->telemetry.gps), sizeof(GPSTelem_t));
				}
			}
      break;
      }
    case CMD_TELEM: { //FIXME!!
			Telemetry_t * telemetry  = (Telemetry_t*)dataPtr;
			Client * tracked_client = tracked_vehicles.getClient(a_client->name);
			if(tracked_client != NULL) {
				memcpy(&(tracked_client->telemetry), telemetry, sizeof(Telemetry_t));
				antenna.tr_telem = 1;
				last_client_update.stamp();
			} else if(a_client == me) {
				/*if(!gs_telem && me->state.gps ){
					gs_telem = true;
					mag_var = WMM_mag_var(me->telemetry.gps.pos.lat, me->telemetry.gps.pos.lon, me->telemetry.gps.pos.alt/1000.0, 2010);
					//memcpy(&last_gps_update, &(me->telemetry.gps), sizeof(GPSTelem_t));
				}

				if(GPSRange(me->telemetry.gps.pos, last_gps_update.pos) > VAR_UPDATE_DIST){
					mag_var = WMM_mag_var(me->telemetry.gps.pos.lat, me->telemetry.gps.pos.lon, me->telemetry.gps.pos.alt/1000.0, 2010);
					//memcpy(&last_gps_update, &(me->telemetry.gps), sizeof(GPSTelem_t));
				}*/
			}
      break;
      }
    case CMD_ACK: {
			  uint8_t id = CMD_DATAPTR(fifo_msg.data)[0];
				if(id == CMD_CONNECT) {
					if(tracked_vehicles.getClient(0) != NULL) {
						Client * a_client = shmPtr->clients.getClient(tracked_vehicles.getClient(0)->name);
						if(a_client != NULL) {
							antenna.tr_back_telem = a_client->state.gs_backup;
						}
					}
				}
			} break;
    case CMD_FAIL: {
			  uint8_t id = CMD_DATAPTR(fifo_msg.data)[0];
				if(id == CMD_CONNECT) {
					antenna.tr_back_telem = 0;
					Client * a_client = shmPtr->clients.getClient(tracked_vehicles.getClient(0)->name);
					if(a_client != NULL)
						a_client->state.gs_backup = 0;
				}
			} break;
    case CMD_ANT_SETTINGS: {
			AntSettings_t * settings  = (AntSettings_t*)dataPtr;
			manual_offset = settings->offset;
			ACK(INBOARD_TX);
			} break;
    case CMD_ANT_STATE: { 
			if(pointing_type == RELATIVE) return; //FIXME hack to keep PAS from responding
			AntState_t * settings  = (AntState_t*)dataPtr;
			
			if((settings->launch_state) == true) {
				if(tracked_vehicles.getClient(0) != NULL) {
					if(!antenna.gs_telem || !antenna.tr_telem || (!antenna.mag_telem && pointing_type == RELATIVE) ) {
						NACK(INBOARD_TX);
					} else {
						ACK(INBOARD_TX);
						TrackTargets(-1.0,20);
						pause_tracking = true;
					}
				}	else {
					NACK(INBOARD_TX);
				}
			} else {
				pause_tracking = false;
				ACK(INBOARD_TX);
			}

			} break;
    default:
      break;
  }
}

void AntennaCmd::TrackTargets(float azimuth, float elevation) {
	if(!antenna.gs_telem || !antenna.tr_telem || (!antenna.mag_telem && pointing_type == RELATIVE) ) {
		cout << "  missing ";
		if(!antenna.gs_telem) cout << "ground station telem ";
		if(!antenna.tr_telem) cout << "tracked vehicle telem ";
		if(!antenna.mag_telem) cout << "magnetometer ";
		cout << endl;

	} else {

		if(tracked_vehicles.getClient(0) != NULL) {
			float	abs_az = GPSBearing(last_gps_update.pos, tracked_vehicles.getClient(0)->telemetry.gps.pos);
			float vehicle_heading = mag.magnetic_heading;

			switch(pointing_type) {
				case ABSOLUTE:
					antenna.azimuth = abs_az;
					break;
				case MAGNETIC:
					antenna.azimuth = abs_az + mag_var;
					break;
				case RELATIVE:
					vehicle_heading = mag.magnetic_heading - mag_var;
					vehicle_heading = fmod(vehicle_heading,360.0);
					if(vehicle_heading < 0) vehicle_heading = 360.0 + vehicle_heading;

					if(vehicle_heading <= abs_az)
						antenna.azimuth = abs_az - vehicle_heading;
					else
						antenna.azimuth = 360.0 - (vehicle_heading - abs_az);
					break;
			}

			antenna.azimuth = antenna.azimuth + manual_offset;

			antenna.azimuth = fmod(antenna.azimuth,360.0);
			if(antenna.azimuth < 0) antenna.azimuth = 360.0 + antenna.azimuth;

			antenna.elevation = RAD_2_DEG * atan2(tracked_vehicles.getClient(0)->telemetry.gps.pos.alt - last_gps_update.pos.alt, GPSDist(last_gps_update.pos, tracked_vehicles.getClient(0)->telemetry.gps.pos));
			if (antenna.elevation < 0) antenna.elevation = 0;

			if(GPSRange(last_gps_update.pos, tracked_vehicles.getClient(0)->telemetry.gps.pos) < omni_range)
				antenna.mode = OMNI;
			else
				antenna.mode = DIRECTIONAL;

			if(azimuth > 0.0) antenna.azimuth = azimuth;
			if(elevation > 0.0) antenna.elevation = elevation;
		}

	}
	Client * me = shmPtr->clients.getClient(0);

	CMD_CMD(fifo_msg.data)  = CMD_ANT_TRACK;
	CMD_NUM(fifo_msg.data)  = sizeof(AntPoint_t);
	CopyStructToBuf(antenna, CMD_DATAPTR(fifo_msg.data) );
	CMD_CSUM(fifo_msg.data) = 0;
	CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

	fifo_msg.size = CMD_SIZE(fifo_msg.data);
	fifo_msg.dest.s_addr = shmPtr->clients.getAddr(requester).s_addr;
	fifo_msg.src.s_addr = shmPtr->clients.getAddr(me).s_addr;
	writeFifo(OUTBOARD_TX);

	CMD_CMD(fifo_msg.data)  = system_type;
	CMD_CSUM(fifo_msg.data) = 0;
	CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);
	writeFifo(INBOARD_TX);
}


float AntennaCmd::WMM_mag_var(float lat_d, float lon_d, float alt_km, float year)
{

	WMMtype_MagneticModel *MagneticModel, *TimedMagneticModel;
	WMMtype_Ellipsoid Ellip;
	WMMtype_CoordSpherical CoordSpherical;
	WMMtype_CoordGeodetic CoordGeodetic;
	WMMtype_Date UserDate;
	WMMtype_GeoMagneticElements GeoMagneticElements;
	WMMtype_Geoid Geoid;
	char filename[] = "/usr/local/share/WMM.COF";
	int NumTerms;

	/* Memory allocation */

	NumTerms = ( ( WMM_MAX_MODEL_DEGREES + 1 ) * ( WMM_MAX_MODEL_DEGREES + 2 ) / 2 );

	MagneticModel 	   = WMM_AllocateModelMemory(NumTerms);
	TimedMagneticModel  = WMM_AllocateModelMemory(NumTerms);
	if(MagneticModel == NULL || TimedMagneticModel == NULL)
	{
		WMM_Error(2);
	}

	WMM_SetDefaults(&Ellip, MagneticModel, &Geoid); /* Set default values and constants */

	WMM_readMagneticModel(filename, MagneticModel);
	WMM_InitializeGeoid(&Geoid);    /* Read the Geoid file */

  /* Set Data for WMM format */
  CoordGeodetic.phi = lat_d;
  CoordGeodetic.lambda = lon_d;
  Geoid.UseGeoid = 1;
  CoordGeodetic.HeightAboveGeoid = alt_km;
  WMM_ConvertGeoidToEllipsoidHeight(&CoordGeodetic, &Geoid);
  UserDate.DecimalYear = year;

	WMM_GeodeticToSpherical(Ellip, CoordGeodetic, &CoordSpherical);
	WMM_TimelyModifyMagneticModel(UserDate, MagneticModel, TimedMagneticModel);
	WMM_Geomag(Ellip, CoordSpherical, CoordGeodetic, TimedMagneticModel, &GeoMagneticElements);
	WMM_CalculateGridVariation(CoordGeodetic,&GeoMagneticElements);

  WMM_FreeMagneticModelMemory(MagneticModel);
  WMM_FreeMagneticModelMemory(TimedMagneticModel);     
  
  if (Geoid.GeoidHeightBuffer)
  {
  	free(Geoid.GeoidHeightBuffer);
  	Geoid.GeoidHeightBuffer = NULL;
  }
  return GeoMagneticElements.Decl;
  
}

void AntennaCmd::PrintMode(TrackerMode mode) {
	String modeStr = "";
	switch(mode) {
		case STANDBY: modeStr = "STANDBY"; break;
		case TRACK: modeStr = "TRACK"; break;
		default: return;
	}
	cout << GREEN << "---------->>>>>      " YELLOW << modeStr << GREEN "      <<<<<----------" << ATTROFF << endl;
}

void AntennaCmd::connectToGS(String ipStr, String portStr, bool connect) {
	CommStruct_t *comm = (CommStruct_t *)CMD_DATAPTR(fifo_msg.data);
	(comm->ip).s_addr = (inet_addr(ipStr.c_str()));
	comm->port = atoi(portStr.c_str());

	comm->connect = connect;

	Client *me = shmPtr->clients.getClient(0);

	fifo_msg.src.s_addr = shmPtr->clients.getAddr(me).s_addr;
	//fifo_msg->dest.s_addr = guiStr.shmPtr->clients.getAddr(me, page_client).s_addr;
	fifo_msg.dest.s_addr = shmPtr->clients.getAddr(me).s_addr;

	CMD_CMD(fifo_msg.data)  = CMD_CONNECT;
	CMD_NUM(fifo_msg.data)  = sizeof(CommStruct_t);
	CMD_CSUM(fifo_msg.data) = 0;
	CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

	fifo_msg.size = CMD_SIZE(fifo_msg.data);
	writeFifo(ThreadedPipe::INBOARD_TX);
}
